#pragma config(Hubs,  S4, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S1,     HTDIR,          sensorNone)
#pragma config(Sensor, S2,     TOUCH,          sensorNone)
#pragma config(Sensor, S3,     HTDIR,          sensorNone)
#pragma config(Sensor, S4,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S4_C1_1,     left_wheel,    tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C1_2,     right_wheel,   tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C2_1,     scissor_lift_up, tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C2_2,     spinner,       tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C3_1,     scissor_lift_down, tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C3_2,     motorI,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S4_C4_1,    grabber_left,         tServoStandard)
#pragma config(Servo,  srvo_S4_C4_2,    grabber_right,        tServoStandard)
#pragma config(Servo,  srvo_S4_C4_3,    hopper,               tServoStandard)
#pragma config(Servo,  srvo_S4_C4_4,    IR_arm,               tServoStandard)
#pragma config(Servo,  srvo_S4_C4_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S4_C4_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.


int cleanup(int x) {
  const int threshold = 10;
  if (x > threshold)
    return (x - threshold) * (x - threshold) /  128;
  if (x < -threshold)
    return (x + threshold) * (x + threshold) / -128;
  return 0;
}

task main()
{
  // waitForStart();
  float irServoDivisor = 10;
  float irServoState = servo[IR_arm]*irServoDivisor;
  float hopperServoDivisor = 10;
  float hopperServoState = servo[hopper]*hopperServoDivisor;
  float grabberLeftServoDivisor = 10;
  float grabberLeftServoState = servo[grabber_left]*grabberLeftServoDivisor;
  float grabberRightServoDivisor = 10;
  float grabberRightServoState = servo[grabber_right]*grabberRightServoDivisor;


  while (true)
  {
    getJoystickSettings(joystick);
		nxtDisplayString(1, "x:%d y:%d", joystick.joy1_x1, joystick.joy1_y1);
		nxtDisplayString(2, "x:%d y:%d", joystick.joy1_x2, joystick.joy1_y2);
		nxtDisplayString(3, "S:%f", irServoState);


    int treadRight  = cleanup(joystick.joy1_y1);
    int treadLeft  = cleanup(joystick.joy1_y2);
    motor[right_wheel] = treadRight;
    motor[left_wheel] = -treadLeft;

    int scissorLift = cleanup(joystick.joy2_y1);
    motor[scissor_lift_up] = scissorLift;

    if ((joystick.joy2_Buttons & 32) > 0) {
			motor[scissor_lift_down] = 50;
		}
		if ((joystick.joy2_Buttons & 128) > 0) {
			motor[scissor_lift_down] = 100;
		}
		if ((joystick.joy2_Buttons & 16) > 0) {
			motor[scissor_lift_down] = -50;
		}
		if ((joystick.joy2_Buttons & 64) > 0) {
			motor[scissor_lift_down] = -100;
		}

		if (((joystick.joy2_Buttons & 32) == 0) &&
			  ((joystick.joy2_Buttons & 128) == 0) &&
			  ((joystick.joy2_Buttons & 16) == 0) &&
			  ((joystick.joy2_Buttons & 64) == 0)) {
			motor[scissor_lift_down] = 0;
		}

		 int hopper = cleanup(joystick.joy2_y2);
		 hopperServoState += (float)hopper/128;
		 if (hopperServoState < 0){
		   hopperServoState = 0;
		 }
		 if (hopperServoState > 255 * hopperServoDivisor){
		   hopperServoState = 255 * hopperServoDivisor;
		 }
		 servo[hopper] = hopperServoState /hopperServoDivisor;

    if ((joystick.joy1_Buttons & 1) > 0) {
			if (irServoState>0){
				irServoState--;
				servo[IR_arm] = irServoState /irServoDivisor;
			}
		}
		if ((joystick.joy1_Buttons & 2) > 0) {
			if (irServoState<(255*irServoDivisor)) {
				irServoState++;
				servo[IR_arm] = irServoState /irServoDivisor;
			}
		}
		if ((joystick.joy2_Buttons & 16) > 0) {
			if (grabberLeftServoState>0){
				grabberLeftServoState--;
				servo[grabber_left] = grabberLeftServoState /grabberLeftServoDivisor;
			}
		}
		if ((joystick.joy2_Buttons & 64) > 0) {
			if (grabberLeftServoState<(255*grabberLeftServoDivisor)) {
				grabberLeftServoState++;
				servo[grabber_left] = grabberLeftServoState /grabberLeftServoDivisor;
			}
		}

		if ((joystick.joy2_Buttons & 32) > 0) {
			if (grabberRightServoState>0){
				grabberRightServoState--;
				servo[grabber_right] = grabberRightServoState /grabberRightServoDivisor;
			}
		}
		if ((joystick.joy2_Buttons & 128) > 0) {
			if (grabberRightServoState<(255*grabberRightServoDivisor)) {
				grabberRightServoState++;
				servo[grabber_right] = grabberRightServoState /grabberRightServoDivisor;
			}
		}
    bFloatDuringInactiveMotorPWM = false;
	}
}
